import json
from pv_agv import AGV
from robot_arm import RobotArm
from camera import Camera
import sys
sys.path.append('.\\lib')
from alg import calcu_base1_t_base2
# C:\Users\K\Desktop\ui\vision_tool\calib\program
from util_3d import *
from lib import xCoreSDK_python
import time
import threading
from scanner import Scanner
from file_sender import FileSender

class SubTask:
    def __init__(self):
        self.agv_map = None
        self.sub_task = None
        self.agv_station = None
        self.obs_arm_project = None
        self.calib_file= None
        self.main_arm_project = None
        self.cam_t_calib_teach_halcon = None
        self.end_t_base_teach_rokea = None
        self.is_correct = True
        self.run_flag = True
        self.ui = None

        self.scanner = None
        self.scanner_project = None
        self.point_template_file = None
        self.point_template = None
        self.default_pose_file = None
        self.default_pose = None
        self.obs_pose = None
        self.scan_pose = None
        # self.file_sender = FileSender("192.168.215.49","WRW","qwer")


    def task_info(self, info_str):
        if self.ui is None:
            print(info_str)
        else:
            self.ui.textBrowserTaskInfo.append(info_str)

    def set_sub_task(self, param_dict):
        self.sub_task = param_dict["sub_task_name"]
        self.agv_station = param_dict["agv_station"]
        self.scanner_project = param_dict["scanner_project"]
        self.scanner.client.open_project(self.scanner_project)
        self.point_template_file = param_dict["point_template"]
        self.default_pose_file = param_dict["default_pose"]


        # self.main_arm_project = param_dict["main_arm_project"]
        # self.obs_arm_project = param_dict["obs_arm_project"]
        # self.is_correct = param_dict["is_correct"]
        # self.calib_file= param_dict["calib_file"]

        with open(self.point_template_file, 'r') as f:
            self.point_template = json.load(f)

        with open(self.default_pose_file, 'r') as f:
            default_pose_dict = json.load(f)
            self.default_pose = default_pose_dict["arm_default_pose"]
            self.obs_pose = default_pose_dict["arm_obs_pose"]
            self.scan_pose = default_pose_dict["arm_scan_pose"]

        # with open(self.calib_file, 'r+') as f:
        #     json_data = json.load(f)
        #     self.end_t_base_teach_rokea = json_data["end_t_base_rokae_type"]
        #     self.cam_t_calib_teach_halcon = json_data["cam_t_calib_halcon_type"]


    def set_agv(self, agv:AGV):
        self.agv = agv

    def set_arm(self, arm:RobotArm):
        self.arm = arm

    def set_cam(self, cam:Camera):
        self.cam = cam

    def load_rokae_project_and_run_block(self, project_name, task_list):
        ec = {}
        self.arm.robot.setMotionControlMode(xCoreSDK_python.MotionControlMode.NrtRLTask, ec)
        self.arm.robot.setOperateMode(xCoreSDK_python.OperateMode.automatic, ec)
        self.arm.robot.setPowerState(True, ec)
        # print("* 设置运行参数, 请依次输入运行速率和是否循环([0]单次/[1]循环), 空格分隔: ", end='')
        self.arm.robot.setProjectRunningOpt(0.5, 0, ec)
        self.arm.robot.loadProject(project_name, task_list, ec)
        self.arm.robot.ppToMain(ec)
        self.arm.robot.runProject(ec)
        while xCoreSDK_python.OperationState.idle != self.arm.robot.operationState(ec):
            pass

    def set_wobj(self, wobj_name, pose_rokae):
        ec = {}
        wobj = xCoreSDK_python.WorkToolInfo()
        wobj.name =  wobj_name #"g_wobj_0"
        wobj.robotHeld =False
        wobj.pos.trans = pose_rokae[:3]
        wobj.pos.rpy = pose_rokae[3:]
        wobj.load.mass = 1
        wobj.load.cog = [0, 0, 0]
        wobj.load.inertia = [0, 0, 0]  # 仔细去找WorkToolInfo结构体中各参数，以及各参数的类型与含义，一个不能错！！！
        self.arm.robot.setWobjInfo(wobj, ec)

    def calcu_wobj(self):
        ec = {}
        self.arm.robot.setDO(0, 0, True, ec)
        # GUI
        # self.cam.update_image()
        time.sleep(1)
        flag, cam_t_calib_halcon = self.cam.gen_cam_t_calib("halcon")
        self.arm.robot.setDO(0, 0, False, ec)
        if flag:
            base_t_end_real_rokae = self.arm.get_end_rokae_type()
            base_t_end_teach_rokae = self.end_t_base_teach_rokea
            end_t_cam_halcon = self.cam.out_param_halcon_type
            cam_t_calib_teach_halcon = self.cam_t_calib_teach_halcon

            cam_t_calib_real = halcon_pose_to_T(cam_t_calib_halcon)
            base_t_end_real = rokae_pose_to_T(base_t_end_real_rokae)
            end_t_cam = halcon_pose_to_T(end_t_cam_halcon)
            cam_t_calib_teach = halcon_pose_to_T(cam_t_calib_teach_halcon)
            base_t_end_teach = rokae_pose_to_T(base_t_end_teach_rokae)

            # 1 real  实际运行
            # 2 teach 示教
            base_t_calib_teach = base_t_end_teach@end_t_cam@cam_t_calib_teach
            base_t_calib_real = base_t_end_real@end_t_cam@cam_t_calib_real
            calib_t_base_teach = trinv(base_t_calib_teach)
            calib_t_base_real = trinv(base_t_calib_real)

            base_real_t_base_teach = base_t_calib_real@calib_t_base_teach

            base_t_calib_teach_rokae = T_to_rokae_pose(base_t_calib_teach)
            base_t_calib_real_rokae = T_to_rokae_pose(base_t_calib_real)
            base_real_t_base_teach_rokae = T_to_rokae_pose(base_real_t_base_teach)

            # print("compare***********")
            # print(base_t_calib_real_rokae)
            # print(base_t_calib_teach_rokae)
            # print(base_real_t_base_teach_rokae)

            # print("function 2")
            # print(base_real_t_base_teach_rokae)
            return  True, base_real_t_base_teach_rokae
        return False, None
    def agv_move(self):
        self.task_info("*AGV 接到任务前往站点:" + self.agv_station)
        self.agv.move_to_station_list([self.agv_station])
        time.sleep(2)
        while self.agv.is_idle() is False:
            time.sleep(1)
            self.task_info("*AGV 前往站点:" + self.agv_station)
        self.task_info("*AGV 到达站点:" + self.agv_station)

    def run(self):
        # step1 move to station
        while not self.run_flag:pass
        self.agv_move()

        if self.is_correct :
            self.task_info("*执行视觉矫正")
            self.task_info("*机械臂运行到观察位置,运行用户示教工程:"+self.obs_arm_project)
            while not self.run_flag: pass
            # step2 run obs project
            print("*机械臂运行到观察位置,运行用户示教工程:"+self.obs_arm_project)
            self.load_rokae_project_and_run_block(self.obs_arm_project, ['task0'])

            # step3 get new wobj
            while not self.run_flag: pass
            ec = {}
            flag, t = self.calcu_wobj()
            if flag:
                self.task_info("*发送新的工件坐标:g_wobj_0->")
                print("*发送新的工件坐标:g_wobj_0->")
                print(t)
                self.set_wobj("g_wobj_0", t)

        # step4 run main project
        self.task_info("*运行用户示教工程:"+self.main_arm_project)
        while not self.run_flag:pass
        self.load_rokae_project_and_run_block(self.main_arm_project, ['task0'])

        # step5 change back to
        self.task_info("*g_wobj_0复位")
        self.set_wobj("g_wobj_0", [0.0,0,0,0,0,0])

    def run_scan_process(self):

        while not self.run_flag:pass
        self.task_info("ARM 移动到默认位置")
        self.arm.movel_rokae(self.default_pose)
        self.task_info("ARM 到达到默认位置")
        self.agv_move()

        self.task_info("ARM 移动到观察位置")
        self.arm.movel_rokae(self.scan_pose)
        self.task_info("ARM 到达到观察位置")
        # move to scan pose
        current_pose = self.arm.get_end_rokae_type()
        delta_list = self.point_template["delta_list"]
        aim_pose_list = []
        for delta in delta_list:
            aim_pose = current_pose[:]
            aim_pose = [current_pose[i] + delta[i] for i in range(0,len(current_pose))]
            aim_pose_list.append(aim_pose)

        num = self.scanner.client.get_total_scan_groups()
        # if num !=len(aim_pose_list):
        #     QMessageBox.warning(self.win, '失败', '打开工程失败', QMessageBox.Yes | QMessageBox.No,
        #                         QMessageBox.Yes)
        for i in range(num):
            # move 移动
            self.arm.movel_rokae(aim_pose_list[i])
            self.scanner.client.start_scan_by_index(i)
            self.task_info("ARM 到达到位置:"+str(i))

        self.scanner.save( "E:\\西工钛现场\\扫描仪箱子_test.asc")
        # self.file_sender.file_send("E:\\西工钛现场\\扫描仪箱子_test.asc", "E:\\test.asc")

        self.task_info("保存文件完成")
        self.task_info("ARM 移动到默认位置")
        self.arm.movel_rokae(self.default_pose)
        self.task_info("ARM 到达到默认位置")


class Task:
    def __init__(self):
        self.sub_task_list = None
        self.name = None
        self.run_flag = True
        self.current_sub_task =None
        self.thread_task = None
        self.ui = None
        self.agv_map = None
        self.scanner = None
        pass

    def set_device(self, arm:RobotArm, cam:Camera, agv:AGV = None, ui = None):
        self.arm = arm
        self.cam = cam
        self.agv = agv
        self.ui = ui
        self.agv.set_current_map(self.agv_map)

    def set_scanner(self, scanner):
        self.scanner = scanner

    def set_task_json(self,json_file):
        with open(json_file, 'r+', encoding="utf-8") as f:
            data = json.load(f)

        self.sub_task_list = data["sub_task_list"]
        self.name = data["task_name"]
        self.run_times = data["run_times"]
        self.agv_map = data["agv_map"]

    def run(self):
        for i in range(self.run_times):
            if self.sub_task_list !=None:
                for sub_task_dict in self.sub_task_list:
                    self.task_info("*子任务:" + sub_task_dict["sub_task_name"])
                    while not self.run_flag:pass
                    self.current_sub_task = SubTask()
                    self.current_sub_task.agv = self.agv
                    self.current_sub_task.arm = self.arm
                    self.current_sub_task.cam = self.cam
                    self.current_sub_task.ui = self.ui
                    self.current_sub_task.scanner = self.scanner
                    self.current_sub_task.set_sub_task(sub_task_dict)
                    self.current_sub_task.run_scan_process()
            self.task_info("*运行次数:"+str(i))


    def pause(self):
        self.current_sub_task.run_flag = False
        self.run_flag = False

    def resume(self):
        self.current_sub_task.run_flag = True
        self.run_flag = True

    def cancel(self):
        self.pause()
        self.sub_task_list = []

    def run_thread(self):
        self.thread_task = threading.Thread(target=self.run)
        self.thread_task.start()

    def task_info(self, info_str):
        if self.ui is None:
            print(info_str)
        else:
            self.ui.textBrowserTaskInfo.append(info_str)


def arm_control():
    arm = RobotArm(IP="192.168.2.160")
    arm_default_pose = arm.get_end_rokae_type()
    jd = {}
    data1 = {"arm_default_pose":arm_default_pose}
    data2 = {"arm_obs_pose":arm_default_pose}
    jd.update(data1)
    jd.update(data2)
    with open("./config/arm_default_pose.json", "w") as f:
        json.dump(jd, f)


def test_task():
    cam = Camera(camera_name="U3V: DA6843249 MV-CU060-10UM")
    arm = RobotArm(IP="192.168.2.160")
    agv = AGV("192.168.100.100","admin","admin")
    scanner = Scanner()
    # t1 = threading.Thread(target=cam.update_image_loop, args=(True,))
    # t1.start()
    task = Task()
    task.set_device(arm, cam, agv)
    task.set_scanner(scanner)
    task.set_task_json("./task_config/json_task3.json")
    task.run()

def test_agv():
    agv = AGV("192.168.100.100","admin","admin")
    agv.move_to_station_list(["g"])

if __name__ == '__main__':
    test_task()
    # arm_control()